import processing.core.PGraphics;
import oscP5.OscP5;
import oscP5.OscIn;

public class Tracker {
  TrackedObject[] myTrackedObjects;
  int blobCount = 0; 
  Vector3f[] blobCoords;
/*------------------------------------------------------------------------------------------------------------------*/
  OscP5 oscP5;   
  String host;
  int sendToPort;
  int receiveAtPort;
  public void initOsc() {
    host = "127.0.0.1";
    sendToPort = 5555;
    receiveAtPort = 3333;
    oscP5 = new OscP5(this, host, sendToPort, receiveAtPort, "getBlobs");
  }
/*------------------------------------------------------------------------------------------------------------------*/
  Tracker(final TrackedObject[] theTrackedObjects) {
    myTrackedObjects = theTrackedObjects;
    blobCoords = new Vector3f[10];
    for (int i = 0; i < blobCoords.length; i++) {
      blobCoords[i] = new Vector3f();
    }
    initOsc();
  }
/*------------------------------------------------------------------------------------------------------------------*/
  public void getBlobs(OscIn oscIn) {
    blobCount = oscIn.getDataList().size() / 4; 
    float blobX, blobY, blobID, blobArea;
      for (int i = 0; i < blobCount; i++) {
        blobX = oscIn.getFloat(i * 4);
        blobX *= (float) stgW;
        blobX += (float) stgW / 2;
        //
        blobY = oscIn.getFloat(i * 4 + 1);
        blobY *= -(float) stgH;
        blobY += (float) stgH / 2;
        //
        blobID = oscIn.getFloat(i * 4 + 2);
        blobArea = oscIn.getFloat(i * 4 + 3);
        //
        blobCoords[i].set(blobX, blobY, blobID, (blobArea*2500));
        //oscIn.print();
        //blobCoords[i].print();
      } 
    }    
/*------------------------------------------------------------------------------------------------------------------*/
  public void checkBlobs() {  
    for (int j = 0; j < myTrackedObjects.length; j++) {
      if (myTrackedObjects[j] != null)
        myTrackedObjects[j].assignedToBlob = false;
    }  
/*------------------------------------------------------------------------------------------------------------------*/
    for (int i = 0; i < blobCoords.length; i++) {
      boolean createTrackedObject = true;
      if (i < blobCount) {
        for (int j = 0; j < myTrackedObjects.length; j++) {
          if (myTrackedObjects[i] != null && myTrackedObjects[i].myID == blobCoords[i].z) {
            myTrackedObjects[i].setPosition(blobCoords[i].x, blobCoords[i].y, 0f, blobCoords[i].w);
            myTrackedObjects[i].assignedToBlob = true; 
            createTrackedObject = false;    
            break;
          }   
        }
        //Call Creation
        if (createTrackedObject)
          createObject(blobCoords[i].x, blobCoords[i].y, (int) blobCoords[i].z, blobCoords[i].w); 
      }
    }  
    //Call Death
    killObjects();
  }
/*------------------------------------------------------------------------------------------------------------------*/
  public void createObject(float theX, float theY, int theObjectID, float theW) {
    for (int i = 0; i < myTrackedObjects.length; i++) {
      if (myTrackedObjects[i] == null) {
        myTrackedObjects[i] = new Cursor(theObjectID);
        myTrackedObjects[i].setPosition(theX, theY, 0, theW);
        myTrackedObjects[i].setStartValues();
        myTrackedObjects[i].assignedToBlob = true;
        break;
      }
    }
  }
/*------------------------------------------------------------------------------------------------------------------*/
  public void killObjects() { 
    //looks through the whole object array
    for (int i = 0; i < myTrackedObjects.length; i++) {
      //if a certain object exists...
      if (myTrackedObjects[i] != null
        && !myTrackedObjects[i].assignedToBlob) {
        //it is to be killed
        myTrackedObjects[i] = null;      
        for (int j=0; j<nr; j++) R[j].picked=false; 
      }
    }
    if (blobCount == 0) {
      for (int i = 0; i < myTrackedObjects.length; i++) {
        //if a certain object exists...     
        if (myTrackedObjects[i] != null) {
          //it is to be killed
          myTrackedObjects[i] = null;        
         
        }
      }
    }
  }
/*------------------------------------------------------------------------------------------------------------------*/
  public void drawDebug(PGraphics g) {  
  g.fill(color(#118D0F),1); 
  g.smooth(); 
  if (blobCount >= 1) {       
  g.fill(255);     
  g.text("Count:" +blobCount, 10, 18); 
  fill(color(#939495),0);
  rect(stgW/2,stgH/2,stgW, stgH);
 //
  for (int i = 1; i < blobCoords.length; i++) {
      // hack for adding distance math
      int t = 1; 
        if (i <= 1) {
          t=i-1;         
          g.stroke(0,255,0,50); 
          g.noFill();
          g.line(blobCoords[i].x, blobCoords[i].y, blobCoords[t].x, blobCoords[t].y);   
          g.stroke(0,255,0,50); 
          float tHold= navDist(blobCoords[i].x, blobCoords[i].y, blobCoords[t].x, blobCoords[t].y);
          g.ellipse(blobCoords[i].x,blobCoords[i].y,tHold*2,tHold*2);
          g.fill(255); 
          g.text(" d:"+tHold,blobCoords[t].x+25,blobCoords[t].y-15);  
          // static north east    
          //g.text("blob" + i + " " + blobCoords[i].x + " " + blobCoords[i].y, 10, 30 + 15 * i); 
        }  
      }
    }    
    g.noSmooth();
  }    
/*------------------------------------------------------------------------------------------------------------------*/
  public float navDist(float theX, float theY, float theX2, float theY2){
    float navRotX=(theX2-theX)*(theX2-theX);
    float navRotY=(theY2-theY)*(theY2-theY);
    return (float) Math.sqrt(navRotX+navRotY);
  }    
  //  
}
